#pragma once
/*
*
*
*/

#include <stdio.h>
#include "Vec3.h"

#ifndef M_PI
#define M_PI 3.14159265f
#endif

namespace Etoile
{
	class Quaternion
	{
	private:
		/*! The internal data representation is private, use operator[] to access values. */
		double q[4];

	public:

		Quaternion()
		{ 
			q[0]=q[1]=q[2]=0.0;  
			q[3]=1.0; 
		}

		Quaternion(const Vec3d& axis, double angle)
		{
			setAxisAngle(axis, angle);
		}

		Quaternion(double  q0, double  q1, double  q2, double  q3)
		{ 
			q[0]=q0;    q[1]=q1;    q[2]=q2;    q[3]=q3; 
		}

		/*! Copy constructor. */
		Quaternion(const Quaternion& Q)
		{ 
			for (int i=0; i<4; ++i) 
			{
				q[i] = Q.q[i]; 
			}
		}

		/*! Sets the Quaternion as a rotation of axis \p axis and angle \p angle (in radians).
		\p axis does not need to be normalized. A null \p axis will result in an identity Quaternion. */
		void setAxisAngle(const Vec3d & axis, double  angle)
		{
			const double norm = axis.length();
			if (norm < 1E-8)
			{
				// Null rotation
				q[0] = 0.0;      q[1] = 0.0;      q[2] = 0.0;      q[3] = 1.0;
			}
			else
			{
				const double  sin_half_angle = sin(angle / 2.0f);
				q[0] = sin_half_angle * axis[0]/norm;
				q[1] = sin_half_angle * axis[1]/norm;
				q[2] = sin_half_angle * axis[2]/norm;
				q[3] = cos(angle / 2.0f);
			}
		}

		/*! Sets the Quaternion value. See the Quaternion(T , T , T , T ) constructor documentation. */
		void setValue(double  q0, double  q1, double  q2, double  q3)
		{ 
			q[0]=q0;    
			q[1]=q1;    
			q[2]=q2;    
			q[3]=q3; 
		}

		void setValue(const Quaternion& Q)
		{ 
			for (int i=0; i<4; ++i)
			{ 
				q[i] = Q.q[i]; 
			}
		}


		/*
		**
		* operator.
		*/
		friend ostream& operator<<(ostream& o, const Quaternion& Q)
		{
			return o << Q[0] << '\t' << Q[1] << '\t' << Q[2] << '\t' << Q[3];
		}

		double  operator[](int i) const 
		{ 
			return q[i]; 
		}

		double & operator[](int i) 
		{ 
			return q[i]; 
		}

		Quaternion operator *(double  scale) const
		{ 
			return Quaternion(q[0]*scale,q[1]*scale,q[2]*scale,q[3]*scale); 
		}

		Quaternion operator /(double  scale) const
		{ 
			return Quaternion(q[0]/scale,q[1]/scale,q[2]/scale,q[3]/scale); 
		}


		Quaternion &operator =(const Quaternion &q)		
		{ 
			this->q[0]= q[0]; 
			this->q[1]= q[1]; 
			this->q[2]=q[2];  
			this->q[3]=q[3];
			return *this; 
		}

		Quaternion operator +(const Quaternion &q) const	
		{ 
			return Quaternion(this->q[0]+q[0], this->q[1]+ q[1], this->q[2]+q[2],  this->q[3]+q[3]);
		}

		Quaternion operator -(const Quaternion &q) const	
		{ 
			return Quaternion(this->q[0]-q[0], this->q[1]-q[1], this->q[2]-q[2], this->q[3]-q[3]); 
		}

		friend Quaternion operator*(const Quaternion& a, const Quaternion& b)
		{
			return Quaternion(a.q[3]*b.q[0] + b.q[3]*a.q[0] + a.q[1]*b.q[2] - a.q[2]*b.q[1],
				a.q[3]*b.q[1] + b.q[3]*a.q[1] + a.q[2]*b.q[0] - a.q[0]*b.q[2],
				a.q[3]*b.q[2] + b.q[3]*a.q[2] + a.q[0]*b.q[1] - a.q[1]*b.q[0],
				a.q[3]*b.q[3] - b.q[0]*a.q[0] - a.q[1]*b.q[1] - a.q[2]*b.q[2]);
		}

		friend Quaternion operator/(const Quaternion& a, const Quaternion& b)
		{
			Quaternion p(b); 
			p.invert(); 
			return a * p;
		}


		bool operator==(const Quaternion &q)
		{
			if(q[3]==q.q[3] && q[2]==q.q[2] &&q[1]==q.q[1] &&q[0]==q.q[0])
				return true;
			return false;
		}

		bool operator!=(const Quaternion &q)
		{
			if(q[3]==q.q[3] && q[2]==q.q[2] &&q[1]==q.q[1] &&q[0]==q.q[0])
				return false;
			return true;
		}

		Quaternion& operator*=(const Quaternion &q)
		{
			*this = (*this)*q;
			return *this;
		}

		Quaternion& operator/=(const Quaternion &q)
		{
			*this = (*this)*q.inverse();
			return *this;
		}

		friend Vec3d  operator*(const Quaternion& q, const Vec3d & v)
		{
			return q.rotate(v);
		}

		double x()
		{
			return q[0];
		}

		double y()
		{
			return q[1];
		}

		double z()
		{
			return q[2];
		}

		double w()
		{
			return q[3];
		}

		//Matrix
		void setFromRotationMatrix(const double m[3][3])
		{
			// Compute one plus the trace of the matrix
			const double  onePlusTrace = 1.0 + m[0][0] + m[1][1] + m[2][2];

			if (onePlusTrace > 1E-5)
			{
				// Direct computation
				const double  s = sqrt(onePlusTrace) * 2.0;
				q[0] = (m[2][1] - m[1][2]) / s;
				q[1] = (m[0][2] - m[2][0]) / s;
				q[2] = (m[1][0] - m[0][1]) / s;
				q[3] = 0.25 * s;
			}
			else
			{
				// Computation depends on major diagonal term
				if ((m[0][0] > m[1][1])&(m[0][0] > m[2][2]))
				{ 
					const double  s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2.0; 
					q[0] = 0.25 * s;
					q[1] = (m[0][1] + m[1][0]) / s; 
					q[2] = (m[0][2] + m[2][0]) / s; 
					q[3] = (m[1][2] - m[2][1]) / s;
				}
				else
					if (m[1][1] > m[2][2])
					{ 
						const double  s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2.0; 
						q[0] = (m[0][1] + m[1][0]) / s; 
						q[1] = 0.25 * s;
						q[2] = (m[1][2] + m[2][1]) / s; 
						q[3] = (m[0][2] - m[2][0]) / s;
					}
					else
					{ 
						const double  s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2.0; 
						q[0] = (m[0][2] + m[2][0]) / s; 
						q[1] = (m[1][2] + m[2][1]) / s; 
						q[2] = 0.25 * s;
						q[3] = (m[0][1] - m[1][0]) / s;
					}
			}
			normalize();
		}


		void setFromRotatedBasis(const Vec3d & X, const Vec3d & Y, const Vec3d & Z)
		{
			double  m[3][3];
			double  normX = X.length();
			double  normY = Y.length();
			double  normZ = Z.length();

			for (int i=0; i<3; ++i)
			{
				m[i][0] = X[i] / normX;
				m[i][1] = Y[i] / normY;
				m[i][2] = Z[i] / normZ;
			}

			setFromRotationMatrix(m);
		}


		//@{
		Vec3d  axis() const
		{
			Vec3d  res = Vec3d(q[0], q[1], q[2]);
			const double sinus = res.length();
			if (sinus > 1E-8)
				res /= sinus;
			return (acos(q[3]) <= M_PI/2.0) ? res : -res;
		}

		double  angle() const
		{
			const double angle = 2.0 * acos(q[3]);
			return (angle <= M_PI) ? angle : 2.0*M_PI - angle;
		}

		void getAxisAngle(Vec3d & axis, double & angle) const
		{
			angle = 2.0*acos(q[3]);
			axis = Vec3d(q[0], q[1], q[2]);
			const double sinus = axis.length();
			if (sinus > 1E-8)
				axis /= sinus;

			if (angle > M_PI)
			{
				angle = 2.0*M_PI - angle;
				axis = -axis;
			}
		}




		Vec3d rotate(const Vec3d & v) const{
			Vec3d vn(v);
			Quaternion vecQuat, resQuat;
			vecQuat[0] = vn.x();
			vecQuat[1] = vn.y();
			vecQuat[2] = vn.z();
			vecQuat[3] = 0.0f;

			resQuat = vecQuat * inverse();
			resQuat = *this * resQuat;
			Vec3d re = Vec3d(resQuat[0], resQuat[1], resQuat[2]);

			return re;
		}

		Vec3d inverseRotate(const Vec3d & v) const
		{
			return inverse().rotate(v);
		}


		Quaternion conjugate( void ) const
		{
			return Quaternion(-q[0], -q[1], -q[2], q[3]);;
		}


		void invert() { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; }

		Quaternion inverse() const 
		{ 
			double  scalar = 1 / ( q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
			return conjugate() * scalar; 
		}


		void negate() { invert(); q[3] = -q[3]; }

		double norm()
		{
			return  (sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]));
		}

		double normalize()
		{
			const double norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
			for (int i=0; i<4; ++i)
				q[i] /= norm;
			return norm;
		}

		Quaternion normalized() const
		{
			double Q[4];
			const double norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
			for (int i=0; i<4; ++i)
			{
				Q[i] = q[i] / norm;
			}
			return Quaternion(Q[0], Q[1], Q[2], Q[3]);
		}



		/*! @name Associated matrix */
		//@{
		const double * matrix() const
		{
			static double  m[4][4];
			getMatrix(m);
			return (const double *)(m);
		}

		void getMatrix(double  m[4][4]) const
		{
			const double  q00 = 2.0l * q[0] * q[0];
			const double  q11 = 2.0l * q[1] * q[1];
			const double  q22 = 2.0l * q[2] * q[2];

			const double  q01 = 2.0l * q[0] * q[1];
			const double  q02 = 2.0l * q[0] * q[2];
			const double  q03 = 2.0l * q[0] * q[3];

			const double  q12 = 2.0l * q[1] * q[2];
			const double  q13 = 2.0l * q[1] * q[3];

			const double  q23 = 2.0l * q[2] * q[3];

			m[0][0] = 1.0l - q11 - q22;
			m[1][0] =        q01 - q23;
			m[2][0] =        q02 + q13;

			m[0][1] =        q01 + q23;
			m[1][1] = 1.0l - q22 - q00;
			m[2][1] =        q12 - q03;

			m[0][2] =        q02 - q13;
			m[1][2] =        q12 + q03;
			m[2][2] = 1.0l - q11 - q00;

			m[0][3] = 0.0l;
			m[1][3] = 0.0l;
			m[2][3] = 0.0l;

			m[3][0] = 0.0l;
			m[3][1] = 0.0l;
			m[3][2] = 0.0l;
			m[3][3] = 1.0l;
		}

		void getMatrix(double m[16]) const
		{
			static double mat[4][4];
			getMatrix(mat);
			int count = 0;
			for (int i=0; i<4; ++i)
				for (int j=0; j<4; ++j)
					m[count++] = mat[i][j];
		}


		void getRotationMatrix(double m[3][3]) const
		{
			static double  mat[4][4];
			getMatrix(mat);
			for (int i=0; i<3; ++i)
				for (int j=0; j<3; ++j)
					// Beware of transposition
					m[i][j] = mat[j][i];
		}

		const double * inverseMatrix() const
		{
			static double  m[4][4];
			getInverseMatrix(m);
			return (const double *)(m);
		}

		void getInverseMatrix(double m[4][4]) const
		{
			inverse().getMatrix(m);
		}

		void getInverseMatrix(double m[16]) const
		{
			inverse().getMatrix(m);
		}

		void getInverseRotationMatrix(double m[3][3]) const
		{
			static double  mat[4][4];
			getInverseMatrix(mat);
			for (int i=0; i<3; ++i)
			{
				for (int j=0; j<3; ++j)
				{
					// Beware of transposition
					m[i][j] = mat[j][i];
				}
			}
		}


		/*! @name linear lerp interpolation */
		static Quaternion lerp(const Quaternion &q1, const Quaternion &q2, double t) 
		{ 
			return (q1*(1-t) + q2*t).normalized(); 
		}

		/*! @name spherical linear Slerp interpolation */
		static Quaternion slerp(const Quaternion& a, const Quaternion& b, double t, bool allowFlip=true)
		{
			double cosAngle = Quaternion::dot(a, b);

			double c1, c2;
			// Linear interpolation for close orientations
			if ((1.0 - fabs(cosAngle)) < 0.01)
			{
				c1 = 1.0f - t;
				c2 = t;
			}
			else
			{
				// Spherical interpolation
				double angle    = acos(fabs(cosAngle));
				double sinAngle = sin(angle);
				c1 = sin(angle * (1.0f - t)) / sinAngle;
				c2 = sin(angle * t) / sinAngle;
			}

			// Use the shortest path
			if (allowFlip && (cosAngle < 0.0))
				c1 = -c1;

			return Quaternion(c1*a[0] + c2*b[0], c1*a[1] + c2*b[1], c1*a[2] + c2*b[2], c1*a[3] + c2*b[3]);
		}

		static Quaternion squad(const Quaternion& a, const Quaternion& tgA, const Quaternion& tgB, const Quaternion& b, double t)
		{
			Quaternion ab = Quaternion::slerp(a, b, t);
			Quaternion tg = Quaternion::slerp(tgA, tgB, t, false);
			return Quaternion::slerp(ab, tg, 2.0*t*(1.0-t), false);
		}


		//! Shoemake-Bezier interpolation using De Castlejau algorithm
		static Quaternion bezier(const Quaternion &q1,const Quaternion &q2,const Quaternion &a,const Quaternion &b, double t)
		{
			// level 1
			Quaternion q11= slerp(q1,a,t),
				q12= slerp(a,b,t),
				q13= slerp(b,q2,t);		
			// level 2 and 3
			return slerp(slerp(q11,q12,t), slerp(q12,q13,t), t);
		}

		//! Given 3 quaternions, qn-1,qn and qn+1, calculate a control point to be used in spline interpolation
		static Quaternion spline(const Quaternion &qnm1,const Quaternion &qn,const Quaternion &qnp1)
		{
			Quaternion qni(qn.inverse());	
			return qn * (( (qni*qnm1).log()+(qni*qnp1).log() )/(-4)).exp();
		}

		static double  dot(const Quaternion& a, const Quaternion& b) 
		{ 
			return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]; 
		}

		Quaternion log()
		{
			double len = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);

			if (len < 1E-6)
				return Quaternion(q[0], q[1], q[2], 0.0);
			else
			{
				double coef = acos(q[3]) / len;
				return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, 0.0);
			}
		}

		Quaternion exp()
		{
			double theta = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);

			if (theta < 1E-6)
				return Quaternion(q[0], q[1], q[2], cos(theta));
			else
			{
				double coef = sin(theta) / theta;
				return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, cos(theta));
			}
		}

		static Quaternion lnDif(const Quaternion& a, const Quaternion& b)
		{
			Quaternion dif = a.inverse()*b;
			dif.normalize();
			return dif.log();
		}

		static Quaternion squadTangent(const Quaternion& before, const Quaternion& center, const Quaternion& after)
		{
			Quaternion l1 = Quaternion::lnDif(center,before);
			Quaternion l2 = Quaternion::lnDif(center,after);
			Quaternion e;
			for (int i=0; i<4; ++i)
			{
				e.q[i] = -0.25 * (l1.q[i] + l2.q[i]);
			}
			e = center*(e.exp());

			// if (Quaternion::dot(e,b) < 0.0)
			// e.negate();

			return e;
		}


		/*! @name Random Quaternion */
		static Quaternion randomQuaternion()
		{
			// The rand() function is not very portable and may not be available on your system.
			// Add the appropriate include or replace by an other random function in case of problem.
			double seed = rand()/(double)RAND_MAX;
			double r1 = sqrt(1.0 - seed);
			double r2 = sqrt(seed);
			double t1 = 2.0 * M_PI * (rand()/(double)RAND_MAX);
			double t2 = 2.0 * M_PI * (rand()/(double)RAND_MAX);
			return Quaternion(sin(t1)*r1, cos(t1)*r1, sin(t2)*r2, cos(t2)*r2);
		}

		// Convert from Z Y X Euler Angles individual RZ - RY - RX,  mulipli RZ* RY * RX   
		void fromEulerXYZ(double roll, double pitch, double yaw)
		{
			double y = yaw  / 2.0;
			double p = pitch  / 2.0;
			double r = roll  / 2.0;

			double sinp = sin(p);
			double siny = sin(y);
			double sinr = sin(r);
			double cosp = cos(p);
			double cosy = cos(y);
			double cosr = cos(r);

			q[0] = sinr * cosp * cosy - cosr * sinp * siny;
			q[1] = cosr * sinp * cosy + sinr * cosp * siny;
			q[2] = cosr * cosp * siny - sinr * sinp * cosy;
			q[3] = cosr * cosp * cosy + sinr * sinp * siny;

			normalize();

		}



		void fromEulerXYZByAngle(double roll, double pitch, double yaw){	
			const double PIOVER180 = 3.14159265 / 180.0;
			double y = yaw * PIOVER180 / 2.0;
			double p = pitch * PIOVER180 / 2.0;
			double r = roll * PIOVER180 / 2.0;

			double sinp = sin(p);
			double siny = sin(y);
			double sinr = sin(r);
			double cosp = cos(p);
			double cosy = cos(y);
			double cosr = cos(r);

			q[0] = sinr * cosp * cosy - cosr * sinp * siny;
			q[1] = cosr * sinp * cosy + sinr * cosp * siny;
			q[2] = cosr * cosp * siny - sinr * sinp * cosy;
			q[3] = cosr * cosp * cosy + sinr * sinp * siny;

			normalize();
		}


		// Convert from Z Y X Euler Angles individual RZ - RY - RX,  mulipli RZ * RY * RX  
		Vec3d  getEulerAngleXYZ()const
		{
			//atan2
			double roll = atan2(2 * (q[3]* q[0] + q[1]* q[2]),   1 - 2*( q[0] * q[0] + q[1] * q[1]));
			double v = q[3] * q[1] - q[2] * q[0];
			if(v > 0.5) v = 0.5;
			if(v < -0.5) v = -0.5;
			double pitch = asin(2* ( v ));
			double yaw = atan2( 2 * ( q[3] * q[2] +  q[0] * q[1]), 1 - 2*( q[1] * q[1] + q[2] * q[2]));

			if(v == 0.5)
			{
				roll = 2 * atan2(q[0], q[3]);
				pitch = M_PI * 0.5;
				yaw = 0;
			}
			else if(v == - 0.5)
			{
				roll = -2 * atan2(q[0], q[3]);
				pitch = -M_PI * 0.5;
				yaw = 0;
			}
			return Vec3d( roll, pitch, yaw);
		}

		Vec3d getEulerAngleXYZBYAngle() const
		{
			const double PIOVER180 =  180.0 /3.14159265;
			Vec3d xyz = getEulerAngleXYZ();
			return Vec3d( xyz.x() * PIOVER180, xyz.y() * PIOVER180, xyz.z() * PIOVER180);
		}

		void fromEulerZYZ(double a, double b, double c)
		{
			q[3] = -cos( (a - c) * 0.5) * sin(b * 0.5);
			q[0] = -sin( (a - c) * 0.5) * sin(b * 0.5);
			q[1] = -sin( (a + c) * 0.5) * cos(b * 0.5);
			q[2] =  sin( (a + c) * 0.5) * cos(b * 0.5);
		}

		Vec3d  getEulerAngleZYZ()const
		{
			double a = atan2( (q[3] *q[1] + q[0]*q[2]), (q[0]*q[1] - q[3]*q[2]));
			double b = acos(-q[3] * q[3] - q[0] *q[0] + q[1] * q[1] + q[2] *q[2]);
			double c = - atan2( (q[3] *q[1] - q[0]*q[2]), (q[0]*q[1] + q[3]*q[2]));
			return Vec3d(a,b,c);
		}

		void fromRotatedPlane(const Vec3d& av, const Vec3d& bv, const Vec3d& a_originalv, const Vec3d& b_originalv)
		{

			Vec3d  n_original = Vec3d::cross3(a_originalv, b_originalv);
			Vec3d  n = Vec3d::cross3(av, bv);

			Vec3d a = av.normalized();
			Vec3d b = bv.normalized();
			Vec3d a_original = a_originalv.normalized();
			Vec3d b_original = b_originalv.normalized();

			n_original.normalize();
			n.normalize();

			Quaternion coplane;
			if(n_original != n)
			{
				Vec3d  intersection = Vec3d::cross3(n_original, n);

				double cosTheta = n_original.dot3(n);
				cosTheta = 1 < cosTheta ? 1 : cosTheta;
				//T  sinTheta = intersection.length();
				intersection.normalize();
				coplane.setAxisAngle(intersection, acos(cosTheta));
			}
			Vec3d a_one = coplane * a_original;
			Vec3d b_one = coplane * b_original;
			Vec3d n_one= Vec3d::cross3(a_one, b_one);
			n_one.normalize();
			//TODO:  can not know which is before which is after by rotation,  maybe wrong direction for rotation
			Quaternion oneplane[4];
			Vec3d  intersection = n;
			//Vec3d  intersection = coplane.inverse() * n;
			//intersection.normalize();
			double adif = a_one.dot3(a);
			//T  bdif = b.dot3(b_original)/ (b.length() * b_original.length() );
			double cosTheta = adif ;//> bdif ? adif : bdif;
			cosTheta = 1 < cosTheta ? 1 : cosTheta; 
			double angle =  acos(cosTheta);
			oneplane[0].setAxisAngle(intersection, -angle);   // PB:here how to decide the angle sign
			oneplane[1].setAxisAngle(intersection, angle); 
			oneplane[2].setAxisAngle(intersection, 3.1415926-angle);   // PB:here how to decide the angle sign
			oneplane[3].setAxisAngle(intersection, 3.1415926+angle); 
			Quaternion final[4];
			Vec3d a_final[4], b_final[4];
			double final_length[4];//,  finalb_length[4];
			int min = 0;
			double minLength = 1000;
			for(int i = 0; i < 4; i++)
			{
				final[i] = (oneplane[i] * coplane);
				//final[i] = (coplane * oneplane[i]);
				a_final[i] = final[i] * a_original;
				b_final[i] = final[i] * b_original;
				final_length[i] = (a_final[i] - a).length();// + (b_final[i] - b).length();
				//finalb_length[i] = (b_final[i] - b).length();
				if(final_length[i] < minLength)
				{
					minLength = final_length[i];
					min = i;
				}
			}
			this->setValue(final[min][0],final[min][1],final[min][2],final[min][3]);
#if defined(_DEBUG) || defined(DEBUG)
			//Vec3d v = getEulerAngleXYZBYAngle();
#endif
			//this->setValue(oneplane_n[0],oneplane_n[1],oneplane_n[2],oneplane_n[3]);

		}


	};

}